//
//  Quaternion.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 11/26/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <math/Quaternion.h>


Quaternion::Quaternion():m_x(0.0),m_y(0.0),m_z(0.0),m_w(0.0)
{
}

Quaternion::Quaternion(double _x, double _y, double _z, double _w):m_x(_x),m_y(_y),m_z(_z),m_w(_w)
{
    double mag = sqrt( m_x * m_x + m_y * m_y + m_z * m_z + m_w * m_w );
    
    m_x /= mag;
    m_y /= mag;
    m_z /= mag;
    m_w /= mag;
}

Quaternion::Quaternion( Sim::Vector _v, double _w):m_x(_v.x()),m_y(_v.y()),m_z(_v.z()),m_w(_w)
{
    double mag = sqrt( m_x * m_x + m_y * m_y + m_z * m_z + m_w * m_w );
    
    m_x /= mag;
    m_y /= mag;
    m_z /= mag;
    m_w /= mag;
}

const Matrix4x4 Quaternion::getMatrix4x4() const
{
    double x2 = m_x * m_x;
    double y2 = m_y * m_y;
    double z2 = m_z * m_z;
    double xy = m_x * m_y;
    double xz = m_x * m_z;
    double yz = m_y * m_z;
    double wx = m_w * m_x;
    double wy = m_w * m_y;
    double wz = m_w * m_z;
    
    return Matrix4x4(1.0 - 2.0*(y2+z2), 2.0*(xy-wz), 2.0*(xz+wy), 0.0,
                     2.0*(xy+wz), 1.0 - 2.0*(x2+z2), 2.0*(yz-wx), 0.0,
                     2.0*(xz-wy), 2.0*(yz+wx), 1.0 - 2.0*(x2+y2), 0.0,
                     0.0, 0.0, 0.0, 1.0);
}

void Quaternion::getAxisAngle(Sim::Vector& axis, double& angle) const
{
    axis.x( m_x );
    axis.y( m_y );
    axis.z( m_z );
    
    axis = axis.unit();
    
    angle = acos(m_w) * 2.0;
}

Quaternion operator*( const Quaternion& a, const Quaternion& b )
{
    Quaternion c;
    
    c.x( a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y() );
    
    c.y( a.w() * b.y() - a.x() * b.z() + a.y() * b.w() + a.z() * b.x() );
    
    c.z( a.w() * b.z() + a.x() * b.y() - a.y() * b.x() + a.z() * b.w() );
    
    c.w( a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z() );
    
    return c;
}

void rotateVector( Sim::Vector& vector, const Sim::Vector& axis, double theta )
{
    Quaternion V( vector , 0.0 );
    
    Quaternion R;
    
    Sim::Vector unit_axis = axis.unit();
    
    double rx = unit_axis.x() * sin(0.5 * theta );
    double ry = unit_axis.y() * sin(0.5 * theta );
    double rz = unit_axis.z() * sin(0.5 * theta );
    
    R.x( rx );
    R.y( ry );
    R.z( rz );
    R.w( cos( 0.5 * theta ) );
    
    R = R.unit();
    
    Quaternion W = R * V * R.conjugate();
    
    vector.x( W.x() );
    vector.y( W.y() );
    vector.z( W.z() );
}

void rotateVector( Sim::Vector& vector, const Quaternion& q )
{
    Quaternion V( vector , 0.0 );
    
    Quaternion W = q * V * q.conjugate();
    
    vector.x( W.x() );
    vector.y( W.y() );
    vector.z( W.z() );
}

Quaternion quatFromAxisAngle( const Sim::Vector& axis, const double angle )
{
    double half_angle = 0.5 * angle;
    
    Sim::Vector unit_axis = axis.unit();
    
    Quaternion q;
    
    q.x( unit_axis.x() * sin(half_angle) );
    q.y( unit_axis.y() * sin(half_angle) );
    q.z( unit_axis.z() * sin(half_angle) );
    q.w( cos( half_angle ) );
    
    q = q.unit();
    
    return q;
}

Quaternion quatFromMatrix4x4( const Matrix4x4& m )
{
    Quaternion q;
    
    double trace = m.m[0][0] + m.m[1][1] + m.m[2][2];
    
    if( trace > 0.0 )
    {
        double s = sqrt( trace + 1.0 );
        q.w( s * 0.5 );
        
        double t = 0.5 / s;
        
        q.x( (m.m[2][1] - m.m[1][2]) * t );
        q.y( (m.m[0][2] - m.m[2][0]) * t );
        q.z( (m.m[1][0] - m.m[0][1]) * t );
    }
    else
    {
        double _q[4];
        
        int i = 0;
        
        if(m.m[1][1] > m.m[0][0] )
        {
            i = 1;
        }
        
        if(m.m[2][2] > m.m[i][i] )
        {
            i = 2;
        }
        
        static const int NEXT[3] = { 1 , 2 , 0 };
        
        int j = NEXT[i];
        int k = NEXT[j];
        
        double s = sqrt( (m.m[i][i] - ( m.m[j][j] + m.m[k][k] )) + 1.0 );
        
        _q[i] = s * 0.5;
        
        double t;
        
        if( s != 0.0 )
        {
            t = 0.5 / s;
        }
        else
        {
            t = s;
        }
        
        _q[3] = ( m.m[k][j] - m.m[j][k] ) * t;
        _q[j] = ( m.m[j][i] - m.m[i][j] ) * t;
        _q[k] = ( m.m[k][i] - m.m[i][k] ) * t;
        
        q.x( _q[0] );
        q.y( _q[1] );
        q.z( _q[2] );
        q.w( _q[3] );
    }
    
    return q;
}
